#include "SensorDataHandler.h"

void vfnSensor_ADCInit (void)
{
    vfnADC_ResolutionSelect (ADC_RESOLUTION_8BIT);
    vfnADC_ConversionMode (ADC_SINGLE_CONVERSION);
    
    vfnADC_PinEnable (SENSOR_ACCEL_X_CHANNEL);
    vfnADC_PinEnable (SENSOR_ACCEL_Y_CHANNEL);
    vfnADC_PinEnable (SENSOR_ACCEL_Z_CHANNEL);
    
    vfnADC_PinEnable (SENSOR_BATTERY_CHANNEL);
    
    vfnADC_PinEnable (SENSOR_DRIVER_TEMP);
    vfnADC_PinEnable (SENSOR_MOTOR_TEMP);
    vfnADC_PinEnable (SENSOR_AMBIENT_TEMP);
    
    vfnADC_PinEnable (SENSOR_HBLED_CURRENT1);
    vfnADC_PinEnable (SENSOR_HBLED_CURRENT2);
    
    vfnADC_SetSampleTime (ADC_LONG_SAMPLE_TIME);
    
    vfnADC_ClockDivideSelect (ADC_CLK_DIVIDER_8);

    vfnADC_LowPowerMode ();
    
    vfnADC_UseBusClock ();
       
}

uint8 u8Sensor_GetBatteryLevel (void)
{
    vfnADC_ChannelSelect (SENSOR_BATTERY_CHANNEL);
    
    while(!u8ADC_CheckCCF());
    
    return u8ADC_GetResult();
}

uint8 u8Sensor_GetAmbientTemp (void)
{
    uint16 u16tempo;
    
    vfnADC_ResolutionSelect (ADC_RESOLUTION_10BIT);
    
    vfnADC_ChannelSelect (SENSOR_AMBIENT_TEMP);
    
    while(!u8ADC_CheckCCF());
    
    u16tempo = u16ADC_GetResult();   
    
    u16tempo -= 150;
    
    u16tempo *= 100;
    
    u16tempo /= 310;
    
    vfnADC_ResolutionSelect (ADC_RESOLUTION_8BIT);
        
    return (uint8)u16tempo;
}

uint8 u8Sensor_GetMotorTemp (void)
{
    vfnADC_ChannelSelect (SENSOR_MOTOR_TEMP);
    
    while(!u8ADC_CheckCCF());
    
    return u8ADC_GetResult();   
}

uint8 u8Sensor_GetDriverTemp (void)
{
    vfnADC_ChannelSelect (SENSOR_DRIVER_TEMP);
    
    while(!u8ADC_CheckCCF());
    
    return u8ADC_GetResult();   
}

uint8 u8Sensor_GetLEDCurrent1 (void)
{
    vfnADC_ChannelSelect (SENSOR_HBLED_CURRENT1);
    
    while(!u8ADC_CheckCCF());
    
    return u8ADC_GetResult();     
}

uint8 u8Sensor_GetLEDCurrent2 (void)
{
    vfnADC_ChannelSelect (SENSOR_HBLED_CURRENT2);
    
    while(!u8ADC_CheckCCF());
    
    return u8ADC_GetResult();     
}

/*
uint8 u8Sensor_GetAccelX (void)
{
    vfnADC_ResolutionSelect (ADC_RESOLUTION_10BIT);
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_X_CHANNEL);
    
    
}

uint8 u8Sensor_GetAccelY (void)
{
    vfnADC_ResolutionSelect (ADC_RESOLUTION_10BIT);
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Y_CHANNEL);
    
    
}

uint8 u8Sensor_GetAccelZ(void)
{
    vfnADC_ResolutionSelect (ADC_RESOLUTION_10BIT);
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Z_CHANNEL);
    
    
}

*/

void vfnSensor_AccelConfig (void)
{
    SENSOR_ACCEL_GSELECT = ACCEL_GSELECT_11G;
    SENSOR_ACCEL_SLEEP = ACCEL_ACTIVE;
    SENSOR_ACCEL_SELFTEST = 0;

    SENSOR_ACCEL_SELFTEST_DD = GPIO_OUTPUT;
    SENSOR_ACCEL_GSELECT_DD = GPIO_OUTPUT;
    SENSOR_ACCEL_SLEEP_DD = GPIO_OUTPUT;
 
}

void vfnSensor_AccelSleep (void)
{
    SENSOR_ACCEL_SLEEP = ACCEL_SLEEP;    
}

void vfnSensor_AccelWakeup (void)
{
    SENSOR_ACCEL_SLEEP = ACCEL_ACTIVE;
}

uint8 u8Sensor_AccelTest (void)
{
    uint16 u16delay;
    uint8 u8AccelStatus = 0;
    uint8 u8tempdata;
    
    uint8 u8prevx = 0;
    uint8 u8prevy = 0;
    uint8 u8prevz = 0;
    
    SENSOR_ACCEL_SELFTEST = ACCEL_SELFTEST_DISABLE;
    u16delay = 0x1000;
    while(u16delay--);
    
    SENSOR_ACCEL_GSELECT = ACCEL_GSELECT_3G;
    
    // Save the previous accelerometer values
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_X_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8prevx = u8ADC_GetResult();
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Y_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8prevy = u8ADC_GetResult();
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Z_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8prevz = u8ADC_GetResult();
    
    SENSOR_ACCEL_SELFTEST = ACCEL_SELFTEST_ENABLE;
    
    /* Wait for at least 5 ms */
    /* This delay is equivalent to approximately 10 ms for a busfreq of 16MHz */
    u16delay = 0x3FFF;
    while(u16delay--);
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_X_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8tempdata = u8ADC_GetResult();
    
    if(u8tempdata > u8prevx)
    {
        u8tempdata -= u8prevx;
    }
    else
    {
        u8tempdata = u8prevx - u8tempdata;
    }
    
    if (u8tempdata < 10)
    {
        u8AccelStatus |= 0x80;    
    }
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Y_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8tempdata = u8ADC_GetResult();
    
    if(u8tempdata > u8prevy)
    {
        u8tempdata -= u8prevy;
    }
    else
    {
        u8tempdata = u8prevy - u8tempdata;
    }
    
    if (u8tempdata < 10)
    {
        u8AccelStatus |= 0x40;
    }
    
    vfnADC_ChannelSelect (SENSOR_ACCEL_Z_CHANNEL);
    while(!u8ADC_CheckCCF());
    u8tempdata = u8ADC_GetResult();
    
    u8tempdata -= u8prevz;
    
    if ((u8tempdata > 23) && (u8tempdata < 46))
    {
        u8AccelStatus |= 0x20;
    }
    
    SENSOR_ACCEL_SELFTEST = ACCEL_SELFTEST_DISABLE;
    
    /* This delay is equivalent to approximately 10 ms for a busfreq of 16MHz */
    u16delay = 0x1FFF;
    while(u16delay--);
     
    return u8AccelStatus; 
        
}














